#include "pid.h"


/**
 * @brief 位置式PID实现
 * 
 * @param pid 
 * @param ActualVal 
 * @return float 
 */
float PositionPID_Realize(PID_t *pid, float ActualVal)
{
    // 获取当前值
    pid->ActualVal = ActualVal;
    // 计算目标值和实际值的差值
    pid->Error = pid->TargetVal - pid->ActualVal;
    // 累计积分项
    pid->Integral += pid->Error;
    // 位置式PID公式
    pid->OutputVal += pid->Kp * pid->Error +                            /* 比例环节 */
                    pid->Ki * pid->Integral +                           /* 积分环节 */
                    pid->Kd * (pid->Error - pid->LastError);            /* 微分环节 */
    // 误差传递
    pid->LastError = pid->Error;
    // 返回输出值
    return pid->OutputVal;
}




/**
 * @brief 增量式PID实现
 * 
 * @param pid 
 * @param ActualVal 
 */
float IncrementPID_Realize(PID_t *pid, float ActualVal)
{
    // 获取当前值
    pid->ActualVal = ActualVal;
    // 计算目标值和实际值的差值
    pid->Error = pid->TargetVal - pid->ActualVal;
    // 增量式PID公式
    pid->OutputVal += pid->Kp * (pid->Error - pid->LastError) +                 /* 比例环节 */
                    pid->Ki * pid->Error +                                      /* 积分环节 */
                    pid->Kd * (pid->Error - 2*pid->LastError + pid->PrevError); /* 微分环节 */
    // 误差传递
    pid->PrevError = pid-> LastError;
    pid->LastError = pid->Error;

    // 返回值限幅
    if(pid->OutputVal > pid->MaxOutputVal)
    {
        pid->OutputVal = pid->MaxOutputVal;
    }
    else if(pid->OutputVal < (-1)*pid->MaxOutputVal)
    {
        pid->OutputVal = (-1)*pid->MaxOutputVal;
    }
    // 返回输出值
    return pid->OutputVal;

}

void set_PID_P_I_D(PID_t *pid,float Kp, float Ki, float Kd)
{
    pid->Kp = Kp;
    pid->Ki = Ki;
    pid->Kd = Kd;
}

void set_PID_TargetVal(PID_t *pid,float TargetVal)
{
    pid->TargetVal = TargetVal;
}

void set_PID_MaxOutputVal(PID_t *pid,float MaxOutputVal)
{
    pid->MaxOutputVal = MaxOutputVal;
}








